import time

import numpy as np

from structures.basic_structure import BasicStructure
from trajectories.traj_base import BaseTrajectory


class CircleTrajectory(BaseTrajectory):
    TRACK_KEY = "circle"

    def get_track_key(self):
        return self.TRACK_KEY

    def get_color(self):
        return [51, 54, 255]

    def tracking(self, args, update_cb, ls: BasicStructure):

        center_point, radius = args

        center_point, radius = ls.forward_ratio(np.array(center_point)), ls.forward_ratio(radius)

        # 半径为10mm时分成20段，以此类推
        slice_count = int(radius * 2)
        sleep_time_second = 1 / slice_count

        circle_slice = np.linspace(0, 2 * np.pi, slice_count)
        print(f"circle -> slice_count: {slice_count} sleep: {sleep_time_second}")

        for radians in circle_slice:
            x, y = center_point[0] + radius * np.cos(radians), center_point[1] + radius * np.sin(radians)
            p = np.array([x, y])

            if ls.try_update_point_end(p):
                alpha, beta = ls.inverse_not_update(p)
                self._add_angles((alpha, beta))
                self._add_point(p)
                update_cb()

                time.sleep(sleep_time_second)

